(require :asdf)
(asdf:operate 'asdf:load-op 'roll-search)
(use-package :roll/search/strips)
(defvar *hakoiri-problem* nil)

;; +---+---+---+---+---+
;; |   |   |   |   |   |
;; +---+---+---+---+---+
;; |   |   |   |   |   |
;; +---+---+---+---+---+
;; |   |   |   |   |   |
;; +---+---+---+---+---+
;; |   |   |   |   |   |
;; +---+---+---+---+---+

(defun init ()
  (setf *hakoiri-problem* (make-instance '<strips-problem>))
  
  (defpredicate *hakoiri-problem* (at (<slider> ?obj) (<cell> ?pos)))
  (defpredicate *hakoiri-problem* (neighbor (<cell> ?pos-a) (<cell> ?pos-b)) :constant t)
  (defpredicate *hakoiri-problem* (cross (<cell> ?pos-a) (<cell> ?pos-b)) :constant t)
  (defpredicate *hakoiri-problem* (empty (<cell> ?pos)))
  
  (defaction *hakoiri-problem*
      :name move-1x1
      :parameters (?target ?from ?to)
      :parameters-type (<1x1> <cell> <cell>)
      :pre-check ((empty ?to)
                  (at ?target ?from)
                  (neighbor ?from ?to))
      :precondition (and (at ?target ?from)
                         (empty ?to)
                         (neighbor ?from ?to))
      :effect (and (empty ?from)
                   (not (empty ?to))
                   (not (at ?target ?from))
                   (at ?target ?to)))
  
  (defaction *hakoiri-problem*
      :name move-up/down-1x2
      :parameters (?target ?from-l ?from-r ?to-l ?to-r)
      :parameters-type (<1x2> <cell> <cell> <cell> <cell>)
      :pre-check ((empty ?to-l)
                  (empty ?to-r)
                  (at ?target ?from-l)
                  (at ?target ?from-r)
                  (neighbor ?to-l ?to-r)
                  (neighbor ?from-l ?to-l)
                  (neighbor ?from-r ?to-r))
      :precondition (and (at ?target ?from-l)
                         (at ?target ?from-r)
                         (empty ?to-l)
                         (empty ?to-r)
                         (neighbor ?from-l ?from-r) ;いらない?
                         (neighbor ?to-l ?to-r)
                         (neighbor ?from-l ?to-l)
                         (neighbor ?from-r ?to-r))
      :effect (and (not (at ?target ?from-l))
                   (not (at ?target ?from-r))
                   (at ?target ?to-l)
                   (at ?target ?to-r)
                   (not (empty ?to-l))
                   (not (empty ?to-r))
                   (empty ?from-l)
                   (empty ?from-r)))
  
  (defaction *hakoiri-problem*
      :name move-left/right-1x2
      :parameters (?target ?from ?center ?to)
      :parameters-type (<1x2> <cell> <cell> <cell>)
      :pre-check ((empty ?to)
                  (at ?target ?from)
                  (at ?target ?center)
                  (neighbor ?to ?center)
                  ;;(not (cross ?to ?from))
                  )
      :precondition (and (at ?target ?from)
                         (at ?target ?center)
                         (empty ?to)
                         (neighbor ?from ?center) ;いらない?
                         (neighbor ?to ?center)
                         (not (cross ?to ?from)))
      :effect (and (not (at ?target ?from))
                   (at ?target ?to)
                   (empty ?from)
                   (not (empty ?to))))
  
  (defaction *hakoiri-problem*
      :name move-left/right-2x1
      :parameters (?target ?from-l ?from-r ?to-l ?to-r)
      :parameters-type (<2x1> <cell> <cell> <cell> <cell>)
      :pre-check ((empty ?to-r)
                  (empty ?to-l)
                  (at ?target ?from-l)
                  (at ?target ?from-r)
                  (neighbor ?to-l ?to-r)
                  (neighbor ?from-l ?to-l)
                  (neighbor ?from-r ?to-r))
      :precondition (and (at ?target ?from-l)
                         (at ?target ?from-r)
                         (empty ?to-l)
                         (empty ?to-r)
                         (neighbor ?from-l ?from-r) ;いらない?
                         (neighbor ?to-l ?to-r)
                         (neighbor ?from-l ?to-l)
                         (neighbor ?from-r ?to-r)
                         )
      :effect (and (not (at ?target ?from-l))
                   (not (at ?target ?from-r))
                   (at ?target ?to-l)
                   (at ?target ?to-r)
                   (not (empty ?to-l))
                   (not (empty ?to-r))
                   (empty ?from-l)
                   (empty ?from-r)))
  
  (defaction *hakoiri-problem*
      :name move-up/down-2x1
      :parameters (?target ?from ?center ?to)
      :parameters-type (<2x1> <cell> <cell> <cell>)
      :pre-check ((empty ?to)
                  (at ?target ?from)
                  (at ?target ?center)
                  (neighbor ?to ?center)
                  ;;(cross ?to ?from)
                  )
      :precondition (and (at ?target ?from)
                         (at ?target ?center)
                         (empty ?to)
                         (neighbor ?from ?center) ;いらない?
                         (neighbor ?to ?center)
                         (not (cross ?to ?from))
                         )
      :effect (and (not (at ?target ?from))
                   (at ?target ?to)
                   (empty ?from)
                   (not (empty ?to))))
  
  (defaction *hakoiri-problem*
    :name move-up/down-2x2
    :parameters (?target ?from-l ?from-r ?center-l ?center-r
                           ?to-l ?to-r)
      :parameters-type (<2x2> <cell> <cell> <cell> <cell> ;これがアウト
                              <cell> <cell>)
      :pre-check ((empty ?to-l)
                  (empty ?to-r)
                  (at ?target ?from-l)
                  (at ?target ?from-r)
                  (at ?target ?center-l)
                  (at ?target ?center-r)
                  (neighbor ?to-l ?to-r)
                  (neighbor ?center-l ?to-l)
                  (neighbor ?to-r ?center-r)
                  ;;(cross ?center-r ?to-l)
                  ;;(cross ?center-l ?to-r)
                  )
      :precondition (and (at ?target ?from-l)
                         (at ?target ?from-r)
                         (at ?target ?center-l)
                         (at ?target ?center-r)
                         (empty ?to-l)
                         (empty ?to-r)
                         ;; from同士が隣
                         (neighbor ?from-l ?from-r)
                         ;; center同士が隣
                         (neighbor ?center-r ?center-l)
                         ;; to同士が隣
                         (neighbor ?to-l ?to-r)
                         ;; l同士が隣
                         (neighbor ?from-l ?center-l)
                         (neighbor ?center-l ?to-l)
                         ;; r同士が隣
                         (neighbor ?from-r ?center-r)
                         (neighbor ?center-r ?to-r)
                         ;; l-rがcross
                         (cross ?from-l ?center-r)
                         (cross ?from-r ?center-l)
                         (cross ?to-l ?center-r)
                         (cross ?to-r ?center-l)
                         )
      :effect (and (not (at ?target ?from-l))
                   (not (at ?target ?from-r))
                   (not (empty ?to-l))
                   (not (empty ?to-r))
                   (at ?target ?to-l)
                   (at ?target ?to-r)
                   (empty ?from-l)
                   (empty ?from-r))
                   )
  
  (defobjects *hakoiri-problem*   
      (<slider>
       <cell>
       (<slider> <1x1>)
       (<slider> <1x2>)
       (<slider> <2x1>)
       (<slider> <2x2>)
       (<1x1> 1x1-a)
       (<1x1> 1x1-b)
       (<1x1> 1x1-c)
       (<1x1> 1x1-d)
       (<1x2> 1x2-a)
       (<1x2> 1x2-b)
       (<1x2> 1x2-c)
       (<1x2> 1x2-d)
       (<2x1> 2x1-a)
       (<2x2> 2x2-a)
       (<cell> 1-1)
       (<cell> 1-2)
       (<cell> 1-3)
       (<cell> 1-4)
       (<cell> 1-5)
       (<cell> 2-1)
       (<cell> 2-2)
       (<cell> 2-3)
       (<cell> 2-4)
       (<cell> 2-5)
       (<cell> 3-1)
       (<cell> 3-2)
       (<cell> 3-3)
       (<cell> 3-4)
       (<cell> 3-5)
       (<cell> 4-1)
       (<cell> 4-2)
       (<cell> 4-3)
       (<cell> 4-4)
       (<cell> 4-5)
       ;;(<cell> goal-1)
       ;;(<cell> goal-2)
       ))
  (definitial-state *hakoiri-problem*
      ( ;; constant conditions
       (neighbor 1-1 1-2)
       (neighbor 1-2 1-3)
       (neighbor 1-3 1-4)
       (neighbor 1-4 1-5)
       (neighbor 2-1 2-2)
       (neighbor 2-2 2-3)
       (neighbor 2-3 2-4)
       (neighbor 2-4 2-5)
       (neighbor 3-1 3-2)
       (neighbor 3-2 3-3)
       (neighbor 3-3 3-4)
       (neighbor 3-4 3-5)
       (neighbor 4-1 4-2)
       (neighbor 4-2 4-3)
       (neighbor 4-3 4-4)
       (neighbor 4-4 4-5)
       (neighbor 1-1 2-1)
       (neighbor 2-1 3-1)
       (neighbor 3-1 4-1)
       (neighbor 1-2 2-2)
       (neighbor 2-2 3-2)
       (neighbor 3-2 4-2)
       (neighbor 1-3 2-3)
       (neighbor 2-3 3-3)
       (neighbor 3-3 4-3)
       (neighbor 1-4 2-4)
       (neighbor 2-4 3-4)
       (neighbor 3-4 4-4)
       (neighbor 1-5 2-5)
       (neighbor 2-5 3-5)
       (neighbor 3-5 4-5)

       (neighbor 1-2 1-1)
       (neighbor 1-3 1-2)
       (neighbor 1-4 1-3)
       (neighbor 1-5 1-4)
       (neighbor 2-2 2-1)
       (neighbor 2-3 2-2)
       (neighbor 2-4 2-3)
       (neighbor 2-5 2-4)
       (neighbor 3-2 3-1)
       (neighbor 3-3 3-2)
       (neighbor 3-4 3-3)
       (neighbor 3-5 3-4)
       (neighbor 4-2 4-1)
       (neighbor 4-3 4-2)
       (neighbor 4-4 4-3)
       (neighbor 4-5 4-4)
       (neighbor 2-1 1-1)
       (neighbor 3-1 2-1)
       (neighbor 4-1 3-1)
       (neighbor 2-2 1-2)
       (neighbor 3-2 2-2)
       (neighbor 4-2 3-2)
       (neighbor 2-3 1-3)
       (neighbor 3-3 2-3)
       (neighbor 4-3 3-3)
       (neighbor 2-4 1-4)
       (neighbor 3-4 2-4)
       (neighbor 4-4 3-4)
       (neighbor 2-5 1-5)
       (neighbor 3-5 2-5)
       (neighbor 4-5 3-5)
       ;; 右下へのcross
       (cross 1-1 2-2)
       (cross 1-2 2-3)
       (cross 1-3 2-4)
       (cross 1-4 2-5)
       (cross 2-1 3-2)
       (cross 2-2 3-3)
       (cross 2-3 3-4)
       (cross 2-4 3-5)
       (cross 3-1 4-2)
       (cross 3-2 4-3)
       (cross 3-3 4-4)
       (cross 3-4 4-5)
       ;; 左下へのcross
       (cross 1-2 2-1)
       (cross 1-3 2-2)
       (cross 1-4 2-3)
       (cross 1-5 2-4)
       (cross 2-2 3-1)
       (cross 2-3 3-2)
       (cross 2-4 3-3)
       (cross 2-5 3-4)
       (cross 3-2 4-1)
       (cross 3-3 4-2)
       (cross 3-4 4-3)
       (cross 3-5 4-4)

       ;; 右下へのcross
       (cross 2-2 1-1)
       (cross 2-3 1-2)
       (cross 2-4 1-3)
       (cross 2-5 1-4)
       (cross 3-2 2-1)
       (cross 3-3 2-2)
       (cross 3-4 2-3)
       (cross 3-5 2-4)
       (cross 4-2 3-1)
       (cross 4-3 3-2)
       (cross 4-4 3-3)
       (cross 4-5 3-4)
       ;; 左下への cros
       (cross 2-1 1-2)
       (cross 2-2 1-3)
       (cross 2-3 1-4)
       (cross 2-4 1-5)
       (cross 3-1 2-2)
       (cross 3-2 2-3)
       (cross 3-3 2-4)
       (cross 3-4 2-5)
       (cross 4-1 3-2)
       (cross 4-2 3-3)
       (cross 4-3 3-4)
       (cross 4-4 3-5)
       
       ;;(neighbor goal-1 goal-2)
       ;;(neighbor 2-5 goal-1)
       ;;(neighbor 3-5 goal-2)
       ;; initial state
       (empty 2-5)
       (empty 3-5)
       (at 2x2-a 2-1)
       (at 2x2-a 2-2)
       (at 2x2-a 3-1)
       (at 2x2-a 3-2)
       (at 1x2-a 1-1)
       (at 1x2-a 1-2)
       (at 1x2-b 1-3)
       (at 1x2-b 1-4)
       (at 1x2-c 4-1)
       (at 1x2-c 4-2)
       (at 1x2-d 4-3)
       (at 1x2-d 4-4)
       (at 2x1-a 2-3)
       (at 2x1-a 3-3)
       (at 1x1-a 1-5)
       (at 1x1-b 2-4)
       (at 1x1-c 3-4)
       (at 1x1-d 4-5)))
  (defgoal-state *hakoiri-problem*
      (and (at 2x2-a 2-5)
           (at 2x2-a 2-4)
           (at 2x2-a 3-5)
           (at 2x2-a 3-4)))
  )

(defun main ()
  (init)
  (let ((solver (make-instance 'roll/search/graph::<a*-graph-search-solver>
                               :problem *hakoiri-problem*
                               :heuristic #'(lambda (x p)
;;                                               (if (eq (car (roll/search/strips::action-of x))
;;                                                       'move-up/down-2x2)
;;                                                   0
;;                                                   1)
                                              0
                                              ))))
    (roll/search/graph:clear-nodes *hakoiri-problem*)
    (setf (roll/search/graph::close-list-of solver) nil)
    (setf (roll/search/graph::open-list-of solver) nil)
    (mapcar #'roll/search/graph::action-of (roll/search/graph::solve solver *hakoiri-problem* :debug nil))
    ))
;; 22 stop?
